﻿using UnityEngine;
using System.Collections;


namespace IMIForUnity
{

    public class Utils
    {
        public static bool isMirror = false;

        //constructs an orientation from a vector that specifies the x axis
        public static Quaternion MakeQuaternionFromX(Vector3 vX, bool flip)
        {
            //matrix columns
            Vector3 xCol;
            Vector3 yCol;
            Vector3 zCol;
            Matrix4x4 jointMatrix4x4;

            //set first column to the vector between the previous joint and the current one, this sets the two degrees of freedom
            xCol = vX.normalized;

            //set second column to an arbitrary vector perpendicular to the first column
            yCol.x = 0.0f;
            yCol.y = !flip ? xCol.z : -xCol.z;
            yCol.z = !flip ? -xCol.y : xCol.y;

            yCol.Normalize();

            //third column is fully determined by the first two, and it must be their cross product
            zCol = Vector3.Cross(xCol, yCol);

            //copy values into matrix
            jointMatrix4x4 = PopulateMatrix(xCol, yCol, zCol);

            return GetQuaternion(jointMatrix4x4);
        }

        //constructs an orientation from 2 vectors: the first specifies the x axis, and the next specifies the y axis
        //uses the first vector as x axis, then constructs the other axes using cross products
        public static Quaternion MakeQuaternionFromXY(Vector3 xUnnormalized, Vector3 yUnnormalized)
        {
            //matrix columns
            Vector3 xCol;
            Vector3 yCol;
            Vector3 zCol;
            Matrix4x4 jointMatrix4x4;

            //set up the three different columns to be rearranged and flipped
            xCol = xUnnormalized.normalized;
            zCol = Vector3.Cross(xCol, yUnnormalized.normalized).normalized;
            yCol = Vector3.Cross(zCol, xCol);

            //copy values into matrix
            jointMatrix4x4 = PopulateMatrix(xCol, yCol, zCol);

            return GetQuaternion(jointMatrix4x4);
        }

        //constructs an orientation from 2 vectors: the first specifies the x axis, and the next specifies the y axis
        //uses the second vector as y axis, then constructs the other axes using cross products
        public static Quaternion MakeQuaternionFromYZ(Vector3 yUnnormalized, Vector3 zUnnormalized)
        {
            //matrix columns
            Vector3 xCol;
            Vector3 yCol;
            Vector3 zCol;
            Matrix4x4 jointMatrix4x4;

            //set up the three different columns to be rearranged and flipped
            yCol = yUnnormalized.normalized;
            xCol = Vector3.Cross(yCol, zUnnormalized.normalized).normalized;
            zCol = Vector3.Cross(xCol, yCol);

            //copy values into matrix
            jointMatrix4x4 = PopulateMatrix(xCol, yCol, zCol);

            return GetQuaternion(jointMatrix4x4);
        }

        //constructs an orientation from 2 vectors: the first specifies the x axis, and the next specifies the y axis
        //uses the second vector as y axis, then constructs the other axes using cross products
        public static Quaternion MakeQuaternionFromYX(Vector3 xUnnormalized, Vector3 yUnnormalized)
        {
            //matrix columns
            Vector3 xCol;
            Vector3 yCol;
            Vector3 zCol;
            Matrix4x4 jointMatrix4x4;

            //set up the three different columns to be rearranged and flipped
            yCol = yUnnormalized.normalized;
            zCol = Vector3.Cross(xUnnormalized.normalized, yCol).normalized;
            xCol = Vector3.Cross(yCol, zCol);

            //copy values into matrix
            jointMatrix4x4 = PopulateMatrix(xCol, yCol, zCol);

            return GetQuaternion(jointMatrix4x4);
        }

        public static Matrix4x4 PopulateMatrix(Vector3 xCol, Vector3 yCol, Vector3 zCol)
        {
            Matrix4x4 jointMatrix4x4 = new Matrix4x4();

            jointMatrix4x4.SetColumn(0, xCol);
            jointMatrix4x4.SetColumn(1, yCol);
            jointMatrix4x4.SetColumn(2, zCol);

            return jointMatrix4x4;
        }

        public static Quaternion GetQuaternion(Matrix4x4 jointMatrix4x4)
        {
            Vector4 vZ = jointMatrix4x4.GetColumn(2);
            Vector4 vY = jointMatrix4x4.GetColumn(1);

            // 镜像
            if (isMirror)
            {
                vZ.y = -vZ.y;
                vY.x = -vY.x;
                vY.z = -vY.z;
            }
            // 非镜像
            else
            {
                vZ.x = -vZ.x;
                vZ.y = -vZ.y;
                vY.z = -vY.z;
            }

            if (vZ.x != 0.0f || vZ.y != 0.0f || vZ.z != 0.0f)
                return Quaternion.LookRotation(vZ, vY);
            else
                return Quaternion.identity;
        }

        /// <summary>
        /// 骨架坐标转换成深度坐标
        /// </summary>
        /// <param name="skeletonPosition">骨架坐标</param>
        /// <param name="depthX">深度X坐标</param>
        /// <param name="depthY">深度Y坐标</param>
        /// <param name="depthZ">深度值</param>
        /// <param name="depthReslution">深度图分辨率</param>
        /// <returns>0表示成功，1表示失败</returns>
        public static void ConvertSkeletonPointToDepthPoint(ImiWrapper.ImiVector4 skeletonPosition, ref int depthX, ref int depthY, ref int depthZ, ImiResolution.Type depthReslution)
        {
            ImiWrapper.ConvertSkeletonPointToDepthPoint(skeletonPosition, ref depthX, ref depthY, ref depthZ, depthReslution);
        }

        /// <summary>
        /// 骨架坐标转换为深度坐标
        /// </summary>
        /// <param name="skeletonPosition">返回深度坐标x,y,z,其中(x,y)坐标即深度图上的坐标，左上角为(0,0)，右下角为(depthWidth,depthHeight)</param>
        /// <returns></returns>
        public static Vector3 ConvertSkeletonPointToDepthPoint(Vector3 skeletonPosition)
        {
            int x = 0, y = 0, z = 0;
            ImiWrapper.ImiVector4 temp = new ImiWrapper.ImiVector4();
            temp.x = skeletonPosition.x;
            temp.y = skeletonPosition.y;
            temp.z = skeletonPosition.z;
            temp.w = 0;
            ImiWrapper.ConvertSkeletonPointToDepthPoint(temp, ref x, ref y, ref z, ImiResolution.Type._640x480);
            return new Vector3(x, y, z);

        }

        /// <summary>
        /// 深度坐标转换成彩色坐标
        /// </summary>
        /// <param name="depthX">深度X坐标</param>
        /// <param name="depthY">深度Y坐标</param>
        /// <param name="depthZ">深度值</param>
        /// <param name="colorX">彩色X坐标</param>
        /// <param name="colorY">彩色Y坐标</param>
        /// <param name="depthReslution">深度图分辨率</param>
        /// <param name="colorReslution">彩色图分辨率</param>
        /// <returns>0表示成功，1表示失败</returns>
        public static void ConvertDepthPointToColorPoint(int depthX, int depthY, int depthZ, ref int colorX, ref int colorY, ImiResolution.Type depthReslution, ImiResolution.Type colorReslution)
        {
            ImiWrapper.ConvertDepthPointToColorPoint(depthX, depthY, depthZ, ref colorX, ref colorY, depthReslution, colorReslution);

        }

        public static Vector2 ConvertDepthPointToColorPoint(Vector3 depthVec)
        {
            int x = 0, y = 0;
            ImiWrapper.ConvertDepthPointToColorPoint((int)depthVec.x, (int)depthVec.y, (int)depthVec.z, ref x, ref y, ImiResolution.Type._640x480, ImiResolution.Type._640x480);
            return new Vector2(x, y);

        }

        /// <summary>
        /// 深度坐标转换成骨架坐标
        /// </summary>
        /// <param name="depthX">深度X坐标</param>
        /// <param name="depthY">深度Y坐标</param>
        /// <param name="depthZ">深度值</param>
        /// <param name="skeletonPosition">骨架坐标</param>
        /// <param name="depthReslution">深度图分辨率</param>
        /// <returns>0表示成功，1表示失败</returns>
        public static void ConvertDepthPointToSkeletonPoint(int depthX, int depthY, int depthZ, ref ImiWrapper.ImiVector4 skeletonPosition, ImiResolution.Type depthReslution)
        {
            ImiWrapper.ConvertDepthPointToSkeletonPoint(depthX, depthY, depthZ, ref skeletonPosition, depthReslution);
        }

        public static Vector3 ConvertDepthPointToSkeletonPoint(Vector3 depthPoint)
        {
            ImiWrapper.ImiVector4 vec = new ImiWrapper.ImiVector4();
            ImiWrapper.ConvertDepthPointToSkeletonPoint((int)depthPoint.x, (int)depthPoint.y, (int)depthPoint.z, ref vec, ImiResolution.Type._640x480);
            return new Vector3(vec.x,vec.y,vec.z);
        }

    }

}
